#include "WPILib.h"
#include "KHSJoystick.h"
#include "GearBox.h"
#include "Jaguar.h"
#include "Encoder.h"
#include "Relay.h"
#include "DigitalOutput.h"
#include "pneumatics.h"
#include "PCVideoServer.h"
#include "Victor.h"
#include "Vision\AxisCamera2010.h"




/**
 * This is a demo program showing the use of the RobotBase class.
 * The SimpleRobot class is the base of a robot application that will automatically call your
 * Autonomous and OperatorControl methods at the right time as controlled by the switches on
 * the driver station or the field controls
 *
 *
 */
class RobotDemo : public SimpleRobot
{
	KHSJoystick *joystick1, *joystick2;
	Gamepad *gamepad;
	GearBox *rightGearBox, *leftGearBox;
	Relay  *v_relay;
	Encoder *rightWheelToothCounter, *leftWheelToothCounter;
	Timer *timer;
	Pneumatics *m_pneumatics;
	Victor *intakeVictor;
	AxisCamera *camera;
	bool m_lastisDisabled;
	bool kicked;
	signed int intakeFlag;
	bool switchDebouncer;
	int pressure;


public:
	RobotDemo(void)
	{

		/**
		 *Joystick1 is paired with rightGearBox
		 *Joystick2 is paired with leftGearBox
		 *
		 */



		GetWatchdog().SetExpiration(400);
		//Joystick objects
		//joystick1 = new KHSJoystick(1);
		joystick2 = new KHSJoystick(2);
		gamepad = new Gamepad(1);

		timer = new Timer();

		rightGearBox = new GearBox(4,1,gamepad,true);
		leftGearBox = new GearBox(4,3,gamepad,false);

		m_pneumatics = new Pneumatics(joystick2);

		camera = &AxisCamera::getInstance();
		camera->writeResolution(k160x120);
		intakeVictor = new Victor(4,5);
		intakeFlag = 0;

		printf("Finished constructing MyRobot Object!!\n");
	}
	void StartCompetition()
	{

		printf("Competition started");

		while (1)
		{
			if (IsAutonomous())
			{
				Autonomous();


			}

			else if (IsOperatorControl())
			{
				OperatorControl();
			}
		}
	}

	void Autonomous(void)
	{
		printf("Starting Autonomous Mode!!\n");
		rightGearBox->m_Encoder->Start();
		leftGearBox->m_Encoder->Start();
		while(IsAutonomous())
		{
			//TODO: Get values for the distance.
			//TODO: Ellipse detection. Last resort: Just having a set angle for each position for
			//turning to goal, or face the ball and goal lined up.
			//TODO: Code for driving the robot to the other side of the  section.
			//TODO: Another autonomous where we allow team mates to score first?
			//TODO: Aligning with goal after ellipses are detected.
			if (!IsDisabled() && m_lastisDisabled)
			{
				timer-> Start();
			}
			m_lastisDisabled = IsDisabled();

			//robot drives forward; receives the ball
			//printf("encoder count %i\n",rightGearBox->m_Encoder->Get());
			if((rightGearBox->m_Encoder->GetDistance()+leftGearBox->m_Encoder->GetDistance())/2<120){
				rightGearBox->drive(0.5);
				leftGearBox->drive(0.5);
			}
			//TODO: change back to else
			if(1){
				rightGearBox->drive(0);
				leftGearBox->drive(0);

			//runs camera; finds target
			/*ColorImage *image;
			EllipseDescriptor whatToLookFor;
			whatToLookFor.maxMajorRadius = 100000;
			whatToLookFor.maxMinorRadius = 100000;
			whatToLookFor.minMajorRadius = 0;
			whatToLookFor.minMinorRadius = 0;
			CurveOptions howToLook;
			//TODO: Fill the CurveOptions structure
			howToLook.extractionMode = IMAQ_NORMAL_IMAGE;
			howToLook.threshold = 50;
			image = camera->GetImage();
			MonoImage *luminancePlane = image->getLuminancePlane();
			vector<EllipseMatch> *results =	luminancePlane->DetectEllipses(&whatToLookFor,&howToLook,NULL,NULL);
			delete image;
			delete luminancePlane;
			printf("found %i ellipses\n",results->size());
			if(results->size() >0){
				printf("found %i ellipses.  #1 position: %f\n",results->size(), results->at(0).position.x);
			}*/
			//robot aligns with found target

			//robot kicks the ball; high pressure in middle and back zones, low pressure in forward zone
				if(!kicked){
					m_pneumatics->kick();
					kicked = true;
				}
			}
			//robot drives to other side of the section

			GetWatchdog().SetEnabled(false);


		}
		printf("Finished Autonomous Mode!!\n");
	}

	/**
	 * Runs the motors with arcade steering.
	 */
	void OperatorControl(void)
	{
		printf("Starting OperatorControl Mode!!\n");
		GetWatchdog().SetEnabled(true);




		//AxisCamera::getInstance();
		//PCVideoServer *pc = new PCVideoServer;
		//pc->Start();

		while (IsOperatorControl())
		{
			Wait(0.05);
			GetWatchdog().Feed();
			m_pneumatics->run();

			//myRobot.ArcadeDrive(stick); // drive with arcade style (use right stick)
			//This part checks to see if the trigger button is down
			//if the trigger is down the robot

			//Sets  victors to whatever they should be according to buttons.
			if(intakeFlag == 1){
				intakeVictor->Set(-1.0f);
			}
			if(intakeFlag == -1){
				intakeVictor->Set(1.0f);
			}

			if(intakeFlag ==0){
				intakeVictor->Set(0);
			}

			//Checks button pressed, sends back.
			if(Gamepad->GetNumberedButton(4)){
				intakeFlag = 1;
			}

			if(Gamepad->GetNumberedButton(2)){
				intakeFlag = -1;
			}

			if(Gamepad->GetNumberedButton(1) || Gamepad->GetNumberedButton(3)){
				intakeFlag = 0;
			}

			if (Gamepad->GetNumberedButton(6))
			{
				leftGearBox->goHalfSpeed(true);
				rightGearBox->goHalfSpeed(true);
			}

			else
			{
				leftGearBox->goHalfSpeed(false);
				rightGearBox->goHalfSpeed(false);
			}

			//if the trigger is down, robot goes half speed.
			rightGearBox->run();
			leftGearBox->run();

		}
		printf("Finished OperatorControl Mode!!\n");
	}


};

START_ROBOT_CLASS(RobotDemo);

